%% Driver code
close all
clear all
clc

data=loadjson('testData1.json')
dataLength = length(data.Data);
MonthNames = {'Jan','Feb','Mar','Apr','May','Jun',...
'Jul','Aug','Sep','Oct','Nov','Dec'};

N=1;
nu0 = 0;
nu = 0;
eta0 = -10^4;
eta = 0;

sigmaT = .01;
sigmaR = .01;
sigmaH = .01;

startPlot = dataLength-400; stopPlot = dataLength-100;
theta =  atan2((nu-nu0),(eta-eta0)) + sigmaT;
r = sqrt((eta - eta0)^2 + (nu-nu0)^2) + sigmaR; 
% Set up model 1 for mode 1: NCV model, 3 sensor measurements dimensional, T=5
% Start with discrete-time (continous time is for wimps)
T = .2;
A1 = [1 T 0 0 0 0 0; ...
       0 1 0 0 0 0 0; ...
       0 0 1 T 0 0 0; ...
       0 0 0 1 0 0 0;...
       0 0 0 0 1 T 0;...
       0 0 0 0 0 1 0;...
       0 0 0 0 0 0 0];
   
Bd1 = [.5*T^2 0 0;...
       T      0 0;...
       0 .5*T^2 0;...
       0 T      0;...
       0 0 .5*T^2;...
       0 0 T     ;... 
       0 0 0     ];
B1=Bd1;   
C1 = [1 0 0 0 0 0 0;... 
      0 0 1 0 0 0 0;... 
      0 0 0 0 1 0 0];

SigmaV11 = r^2*exp(-2*sigmaT^2)*((cos(theta))^2*(cosh(2*sigmaT^2) - cosh(sigmaT^2))...
                +(sin(theta))^2*(sinh(2*sigmaT^2) - sinh(sigmaT^2))) + ...
                +sigmaR^2*exp(-2*sigmaT^2)*((cos(theta))^2*(2*cosh(2*sigmaT^2) - cosh(sigmaT^2))...
                +(sin(theta))^2*(2*sinh(2*sigmaT^2) - sinh(sigmaT^2)));
            
SigmaV12 = sin(theta)*cos(theta)*exp(-4*sigmaT^2)*(sigmaR^2 + (r^2 + sigmaR^2)*(1-exp(sigmaT^2)));

SigmaV22 = r^2*exp(-2*sigmaT^2)*((sin(theta))^2*(cosh(2*sigmaT^2) - cosh(sigmaT^2))...
                + (cos(theta))^2*(sinh(2*sigmaT^2) - sinh(sigmaT^2))) ...
                + sigmaR^2*exp(-2*sigmaT^2)*((sin(theta))^2*(2*cosh(2*sigmaT^2) - cosh(sigmaT^2))...
                + (cos(theta))^2*(2*sinh(2*sigmaT^2) - sinh(sigmaT^2)));

Sv1 = [sigmaR 0 0; 0 sigmaT 0; 0 0 sigmaH]; %measurement noise
Sw1 = Bd1*diag([.5^2 .5^2 .5^5])*Bd1'; % process noise
SX =  Sw1;
SX1 = SX(:);

%% Set up model 2 for mode 2: NCP model, one dimensional, 
% Start with continuous-time; convert to discrete-time
Ac = zeros(7); 
Bc = [1 0 0;...
      0 0 0;...
      0 1 0;...
      0 0 0;
      0 0 1;...
      0 0 0;...
      0 0 0]; 

[A2 B2] = c2d(Ac,Bc,T);
C2 = [1 0 0 0 0 0 0;... 
      0 0 1 0 0 0 0;... 
      0 0 0 0 1 0 0]; 
Sw2 = B2*diag([5^2 5^2 5^2])*B2'; % process noise
Sv2 = [sigmaR 0 0; 0 sigmaT 0; 0 0 sigmaH]; %measurement noise
SX =  Sw2;
SX2 = SX(:);

%% Populate the immData and immState structures
immData = [];
immData.txprob = [0.25 0.75; 0.25 0.75];
immData.A(:,:,1) = A1; immData.A(:,:,2) = A2;
immData.B(:,:,1) = B1; immData.B(:,:,2) = B2;
immData.C(:,:,1) = C1; immData.C(:,:,2) = C2;
immData.D(:,:,1) = 0; immData.D(:,:,2) = 0; 
immData.Sw(:,:,1) = Sw1; immData.Sw(:,:,2) = Sw2;
immData.Sv(:,:,1) = Sv1; immData.Sv(:,:,2) = Sv2;

immState = [];
immState.mode = [0.25; 0.75]; % a-priori likelihood for each mode
immState.X = [67,66;2,0.5;44,44;-0.8,-0.2;23,23;-0.1,-0.4;0,0]; % initialize both modes to same state estimate
immState.SX = [0.009,0.003;
               0.013,0.004;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.013,0.004;
               0.066,0.064;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,0.008;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.009,0.003;
               0.013,0.004;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,-0.001;
               0.013,0.004;
               0.066,0.057;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.000,0.000;
               0.009,0.005;
               0.008,0.007;
               0.000,0.000;
               0.000,0.000;
               0.000,0.008;
               0.000,0.000;
               0.000,0.000;
               0.008,0.007;
               0.022,0.075;
               0,0;
               0,0;
               0,0;
               0,0;
               0,0;
               0,0;
               0,0;
               0,0];
% immState.SX = [SX1 SX2];
immState.xhat = [67;0;4;0;23;0;0]; % these two are outputs, not inputs, but I'll set
immState.SigmaX = [0.009,0.012,0,    0,   0,    0,    0; 
                   0.012,0.069,0,    0,   0,    0,    0;
                   0,    0,    0.009,0.01,0,    0,    0;
                   0,    0,    0.012,0.06,0,    0,    0;
                   0,    0,    0,    0,   0.009,0.008,0;
                   0,    0,    0,    0,   0.008,0.022,0;
                   0,    0,    0,    0,   0,    0,    0]; % them anyway

%% Generate the true system data
xtrue = zeros([7,dataLength+1]); 
z = zeros(3,dataLength);
t=zeros([1,dataLength+1]);

for k = 1:length(data.Data),
    xtrue(:,k) = [str2num(data.Data{1,k}.Sensor.T) xtrue(2,k) str2num(data.Data{1,k}.Sensor.L) xtrue(4,k) str2num(data.Data{1,k}.Sensor.H) xtrue(6,k) xtrue(7,k)];
    xtrue(:,k+1) = A1*xtrue(:,k);
    z(:,k) = C1*xtrue(:,k+1);
    
    monthStr = data.Data{1,k}.Sensor.Time(5:7);
    monthNum = find(ismember(MonthNames, monthStr));
    dayNum = str2double(data.Data{1,k}.Sensor.Time(9:11));
    yearNum = str2double(data.Data{1,k}.Sensor.Time(12:16));
    hourNum = str2double(data.Data{1,k}.Sensor.Time(17:18));
    minNum = str2double(data.Data{1,k}.Sensor.Time(20:21));
    secondNum = str2double(data.Data{1,k}.Sensor.Time(23:24));

    t(k) = datenum(yearNum,monthNum,dayNum,hourNum,minNum,secondNum);

end

figure(1); clf; plot(t(startPlot:stopPlot),xtrue(1,startPlot:stopPlot),'k'); hold on;
datetick('x', 'dd mmm yy', 'keeplimits', 'keepticks')
xlabel('measurement time');
ylabel('temperature (F)');     
grid on

figure(4); clf; plot3(xtrue(3,startPlot:stopPlot),xtrue(1,startPlot:stopPlot),xtrue(5,startPlot:stopPlot),'k'); hold on;
xlabel('light (%)');
ylabel('temperature (F)');   
zlabel('humidity (%)');
grid on

figure(10); hold on; plot(t(startPlot:stopPlot),xtrue(3,startPlot:stopPlot),'k');
datetick('x', 'dd mmm yy', 'keeplimits', 'keepticks')
xlabel('measurement time');
ylabel('light intensity (%)');     
grid on

figure(11); hold on; plot(t(startPlot:stopPlot),xtrue(5,startPlot:stopPlot),'k');
datetick('x', 'dd mmm yy', 'keeplimits', 'keepticks')
xlabel('measurement time');
ylabel('humidity (%)');     
grid on

%% Run the IMM on this data
xhatstore = zeros([7,dataLength+1]);
modestore = zeros([2,dataLength]);
sigmastore = zeros([49,dataLength+1]);
for k = 1:dataLength,
    immState = iterIMM([0;0;0],z(:,k),immState,immData);
    xhatstore(:,k+1) = immState.xhat;
    sigmastore(:,k+1) = immState.SigmaX(:);
    modestore(:,k) = immState.mode;
end


figure(1);
hold on; plot(t(startPlot:stopPlot),xhatstore(1,startPlot:stopPlot),'r');
datetick('x', 'dd mmm yy', 'keeplimits', 'keepticks')
title('True and estimated temperature');
legend('actual','estimate')
grid on
figure(4); clf; plot3(xhatstore(3,startPlot:stopPlot),xhatstore(1,startPlot:stopPlot),xhatstore(5,startPlot:stopPlot),'r'); hold on;
xlabel('light (%)');
ylabel('temperature (F)');   
zlabel('humidity (%)');
title('True and estimated system values');
grid on

figure(10);
hold on; plot(t(startPlot:stopPlot),xhatstore(3,startPlot:stopPlot),'r');
datetick('x', 'dd mmm yy', 'keeplimits', 'keepticks')
title('True and estimated light intensity');
legend('actual','estimate')
grid on

figure(11);
hold on; plot(t(startPlot:stopPlot),xhatstore(5,startPlot:stopPlot),'r');
datetick('x', 'dd mmm yy', 'keeplimits', 'keepticks')
title('True and estimated humidity');
legend('actual','estimate')
grid on

figure(2);
clf; plot((xtrue(1,startPlot:stopPlot)-xhatstore(1,startPlot:stopPlot))); hold on;
plot(3*sqrt(sigmastore(1,startPlot:stopPlot)),'k--','linewidth',0.5);
plot(-3*sqrt(sigmastore(1,startPlot:stopPlot)),'k--','linewidth',0.5);
datetick('x', 'dd mmm yy', 'keeplimits', 'keepticks')
title('Tracking error and bounds for temperature');
xlabel('measurement number'); ylabel('Error (F)');
grid on;
axis([0 length(xtrue(1,startPlot:stopPlot)) -5 5]);

figure(3);
clf; plot(modestore(:,startPlot:stopPlot)');
hold on; plot([0 length(startPlot:stopPlot)],[0.5 0.5],'k--','linewidth',0.5);
grid on;
title('Estimated mode PMF values for the system');
xlabel('measurement number'); ylabel('Probability');
legend('NCV', 'NCP')
axis([0 length(modestore(startPlot:stopPlot)) -0.05 1.05]);

figure(20); 
clf; plot(xtrue(3,startPlot:stopPlot)-xhatstore(3,startPlot:stopPlot)); hold on;
plot(3*sqrt(sigmastore(25,startPlot:stopPlot)),'k--','linewidth',0.5);
plot(-3*sqrt(sigmastore(25,startPlot:stopPlot)),'k--','linewidth',0.5);
grid on;
title('Tracking error and bounds for light');
xlabel('measurement number'); ylabel('Error (% Light)');
axis([0 length(xtrue(3,startPlot:stopPlot)) -10 10]);

figure(21); 
clf; plot(xtrue(5,startPlot:stopPlot)-xhatstore(5,startPlot:stopPlot)); hold on;
plot(3*sqrt(sigmastore(41,startPlot:stopPlot)),'k--','linewidth',0.5);
plot(-3*sqrt(sigmastore(41,startPlot:stopPlot)),'k--','linewidth',0.5);
grid on;
title('Tracking error and bounds for humidity');
xlabel('measurement number'); ylabel('Error (% Humidity)');
axis([0 length(xtrue(5,startPlot:stopPlot)) -5 5]);






